home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / linalg / lu.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-04-18  |  7.1 KB  |  313 lines
  1. /* linalg/lu.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. /* Author:  G. Jungman */
  21.  
  22. #include <config.h>
  23. #include <stdlib.h>
  24. #include <string.h>
  25. #include <gsl/gsl_math.h>
  26. #include <gsl/gsl_vector.h>
  27. #include <gsl/gsl_matrix.h>
  28. #include <gsl/gsl_permute_vector.h>
  29. #include <gsl/gsl_blas.h>
  30.  
  31. #include <gsl/gsl_linalg.h>
  32.  
  33. #define REAL double
  34.  
  35. /* Factorise a general N x N matrix A into,
  36.  *
  37.  *   P A = L U
  38.  *
  39.  * where P is a permutation matrix, L is unit lower triangular and U
  40.  * is upper triangular.
  41.  *
  42.  * L is stored in the strict lower triangular part of the input
  43.  * matrix. The diagonal elements of L are unity and are not stored.
  44.  *
  45.  * U is stored in the diagonal and upper triangular part of the
  46.  * input matrix.  
  47.  * 
  48.  * P is stored in the permutation p. Column j of P is column k of the
  49.  * identity matrix, where k = permutation->data[j]
  50.  *
  51.  * signum gives the sign of the permutation, (-1)^n, where n is the
  52.  * number of interchanges in the permutation. 
  53.  *
  54.  * See Golub & Van Loan, Matrix Computations, Algorithm 3.4.1 (Gauss
  55.  * Elimination with Partial Pivoting).
  56.  */
  57.  
  58. int
  59. gsl_linalg_LU_decomp (gsl_matrix * A, gsl_permutation * p, int *signum)
  60. {
  61.   if (A->size1 != A->size2)
  62.     {
  63.       GSL_ERROR ("LU decomposition requires square matrix", GSL_ENOTSQR);
  64.     }
  65.   else if (p->size != A->size1)
  66.     {
  67.       GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
  68.     }
  69.   else
  70.     {
  71.       const size_t N = A->size1;
  72.       size_t i, j, k;
  73.  
  74.       *signum = 1;
  75.       gsl_permutation_init (p);
  76.  
  77.       for (j = 0; j < N - 1; j++)
  78.     {
  79.       /* Find maximum in the j-th column */
  80.  
  81.       REAL ajj, max = fabs (gsl_matrix_get (A, j, j));
  82.       size_t i_pivot = j;
  83.  
  84.       for (i = j + 1; i < N; i++)
  85.         {
  86.           REAL aij = fabs (gsl_matrix_get (A, i, j));
  87.  
  88.           if (aij > max)
  89.         {
  90.           max = aij;
  91.           i_pivot = i;
  92.         }
  93.         }
  94.  
  95.       if (i_pivot != j)
  96.         {
  97.           gsl_matrix_swap_rows (A, j, i_pivot);
  98.           gsl_permutation_swap (p, j, i_pivot);
  99.           *signum = -(*signum);
  100.         }
  101.  
  102.       ajj = gsl_matrix_get (A, j, j);
  103.  
  104.       if (ajj != 0.0)
  105.         {
  106.           for (i = j + 1; i < N; i++)
  107.         {
  108.           REAL aij = gsl_matrix_get (A, i, j) / ajj;
  109.           gsl_matrix_set (A, i, j, aij);
  110.  
  111.           for (k = j + 1; k < N; k++)
  112.             {
  113.               REAL aik = gsl_matrix_get (A, i, k);
  114.               REAL ajk = gsl_matrix_get (A, j, k);
  115.               gsl_matrix_set (A, i, k, aik - aij * ajk);
  116.             }
  117.         }
  118.         }
  119.     }
  120.       
  121.       return GSL_SUCCESS;
  122.     }
  123. }
  124.  
  125. int
  126. gsl_linalg_LU_solve (const gsl_matrix * LU, const gsl_permutation * p, const gsl_vector * b, gsl_vector * x)
  127. {
  128.   if (LU->size1 != LU->size2)
  129.     {
  130.       GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
  131.     }
  132.   else if (LU->size1 != p->size)
  133.     {
  134.       GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
  135.     }
  136.   else if (LU->size1 != b->size)
  137.     {
  138.       GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
  139.     }
  140.   else if (LU->size2 != x->size)
  141.     {
  142.       GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
  143.     }
  144.   else
  145.     {
  146.       /* Copy x <- b */
  147.  
  148.       gsl_vector_memcpy (x, b);
  149.  
  150.       /* Solve for x */
  151.  
  152.       gsl_linalg_LU_svx (LU, p, x);
  153.  
  154.       return GSL_SUCCESS;
  155.     }
  156. }
  157.  
  158.  
  159. int
  160. gsl_linalg_LU_svx (const gsl_matrix * LU, const gsl_permutation * p, gsl_vector * x)
  161. {
  162.   if (LU->size1 != LU->size2)
  163.     {
  164.       GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
  165.     }
  166.   else if (LU->size1 != p->size)
  167.     {
  168.       GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
  169.     }
  170.   else if (LU->size1 != x->size)
  171.     {
  172.       GSL_ERROR ("matrix size must match solution/rhs size", GSL_EBADLEN);
  173.     }
  174.   else
  175.     {
  176.       /* Apply permutation to RHS */
  177.  
  178.       gsl_permute_vector (p, x);
  179.  
  180.       /* Solve for c using forward-substitution, L c = P b */
  181.  
  182.       gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasUnit, LU, x);
  183.  
  184.       /* Perform back-substitution, U x = c */
  185.  
  186.       gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LU, x);
  187.  
  188.       return GSL_SUCCESS;
  189.     }
  190. }
  191.  
  192.  
  193. int
  194. gsl_linalg_LU_refine (const gsl_matrix * A, const gsl_matrix * LU, const gsl_permutation * p, const gsl_vector * b, gsl_vector * x, gsl_vector * residual)
  195. {
  196.   if (A->size1 != A->size2)
  197.     {
  198.       GSL_ERROR ("matrix a must be square", GSL_ENOTSQR);
  199.     }
  200.   if (LU->size1 != LU->size2)
  201.     {
  202.       GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
  203.     }
  204.   else if (A->size1 != LU->size2)
  205.     {
  206.       GSL_ERROR ("LU matrix must be decomposition of a", GSL_ENOTSQR);
  207.     }
  208.   else if (LU->size1 != p->size)
  209.     {
  210.       GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
  211.     }
  212.   else if (LU->size1 != b->size)
  213.     {
  214.       GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
  215.     }
  216.   else if (LU->size1 != x->size)
  217.     {
  218.       GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
  219.     }
  220.   else
  221.     {
  222.       /* Compute residual, residual = (A * x  - b) */
  223.  
  224.       gsl_vector_memcpy (residual, b);
  225.       gsl_blas_dgemv (CblasNoTrans, 1.0, A, x, -1.0, residual);
  226.  
  227.       /* Find correction, delta = - (A^-1) * residual, and apply it */
  228.  
  229.       gsl_linalg_LU_svx (LU, p, residual);
  230.       gsl_blas_daxpy (-1.0, residual, x);
  231.  
  232.       return GSL_SUCCESS;
  233.     }
  234. }
  235.  
  236. int
  237. gsl_linalg_LU_invert (const gsl_matrix * LU, const gsl_permutation * p, gsl_matrix * inverse)
  238. {
  239.   size_t i, n = LU->size1;
  240.  
  241.   int status = GSL_SUCCESS;
  242.  
  243.   gsl_matrix_set_identity (inverse);
  244.  
  245.   for (i = 0; i < n; i++)
  246.     {
  247.       gsl_vector_view c = gsl_matrix_column (inverse, i);
  248.       int status_i = gsl_linalg_LU_svx (LU, p, &(c.vector));
  249.  
  250.       if (status_i)
  251.     status = status_i;
  252.     }
  253.  
  254.   return status;
  255. }
  256.  
  257. double
  258. gsl_linalg_LU_det (gsl_matrix * LU, int signum)
  259. {
  260.   size_t i, n = LU->size1;
  261.  
  262.   double det = (double) signum;
  263.  
  264.   for (i = 0; i < n; i++)
  265.     {
  266.       det *= gsl_matrix_get (LU, i, i);
  267.     }
  268.  
  269.   return det;
  270. }
  271.  
  272.  
  273. double
  274. gsl_linalg_LU_lndet (gsl_matrix * LU)
  275. {
  276.   size_t i, n = LU->size1;
  277.  
  278.   double lndet = 0.0;
  279.  
  280.   for (i = 0; i < n; i++)
  281.     {
  282.       lndet += log (fabs (gsl_matrix_get (LU, i, i)));
  283.     }
  284.  
  285.   return lndet;
  286. }
  287.  
  288.  
  289. int
  290. gsl_linalg_LU_sgndet (gsl_matrix * LU, int signum)
  291. {
  292.   size_t i, n = LU->size1;
  293.  
  294.   int s = signum;
  295.  
  296.   for (i = 0; i < n; i++)
  297.     {
  298.       double u = gsl_matrix_get (LU, i, i);
  299.  
  300.       if (u < 0)
  301.     {
  302.       s *= -1;
  303.     }
  304.       else if (u == 0)
  305.     {
  306.       s = 0;
  307.       break;
  308.     }
  309.     }
  310.  
  311.   return s;
  312. }
  313.